﻿using UnityEngine;
using System.Collections.Generic;
using System.Collections;
using System;
using System.Runtime.CompilerServices;

namespace IMIForUnity
{
    public class ImiTexture2D
    {

        public enum Type
        {
            COLOR,
            DEPTH,
            USER_DEPTH,
            MAIN_USER_COLOR,
            SUB_USER_COLOR,
            MAIN_USER_DEPTH,
            SUB_USER_DEPTH
        }
        protected Type type;

        protected byte[] textureData;
        protected Texture2D texture;
        protected ImiManager imiManager;
        protected ImiTextureDataHolder dataHolder;
        protected ImiResolution resolution;
        public Texture2D Texture { get { return texture; } }

        internal ImiTexture2D(Type type, ImiResolution resolution)
        {
            this.type = type;
            if (type == Type.COLOR)
            {
                texture = new Texture2D(resolution.Width, resolution.Height, TextureFormat.RGB24, false);
            }
            else
            {
                texture = new Texture2D(resolution.Width, resolution.Height, TextureFormat.RGBA32, false);
            }
            imiManager = ImiManager.GetInstance();
            dataHolder = ImiTextureDataHolder.GetInstance();
            this.resolution = resolution;
        }

        internal void LoadTextureData()
        {
            switch (type)
            {
                case Type.COLOR:
                    textureData = dataHolder.colorDataRaw;
                    break;
                case Type.DEPTH:
                    textureData = dataHolder.depthData;
                    break;
                case Type.MAIN_USER_COLOR:
                    textureData = dataHolder.mainUserColorData;
                    break;
                case Type.MAIN_USER_DEPTH:
                    textureData = dataHolder.mainUserDepthData;
                    break;
                case Type.SUB_USER_COLOR:
                    textureData = dataHolder.subUserColorData;
                    break;
                case Type.SUB_USER_DEPTH:
                    textureData = dataHolder.subUserDepthData;
                    break;
                case Type.USER_DEPTH:
                    textureData = dataHolder.allUserDepthData;
                    break;
            }
        }

        internal void UpdateTexture(byte[] textureData)
        {
            if (type == Type.COLOR)
            {
                LoadTextureData();
                if (textureData != null)
                {
                    texture.LoadRawTextureData(textureData);
                    texture.Apply();
                }
            }
            else
            {
                if (dataHolder.dataReady)
                {
                    LoadTextureData();
                    if (textureData != null)
                    {
                        texture.LoadRawTextureData(textureData);
                        texture.Apply();
                    }

                }
            }
        }

        internal void ReleaseTexture()
        {
            texture = null;
            textureData = null;
        }
    }


    internal class ImiTexture2DFactory
    {

        public static ImiTexture2D CreateTexture(ImiTexture2D.Type type)
        {
            ImiPreferences imiPreferences = ImiPreferences.GetInstance();
            ImiTexture2D texture2d = null;
            switch (type)
            {
                case ImiTexture2D.Type.COLOR:
                    imiPreferences.showAllUsersColor = true;
                    texture2d = new ImiTexture2D(type, imiPreferences.colorResolution);
                    break;
                case ImiTexture2D.Type.DEPTH:
                    imiPreferences.showDepthOnly = true;
                    texture2d = new ImiTexture2D(type, imiPreferences.depthResolution);
                    break;
                case ImiTexture2D.Type.USER_DEPTH:
                    imiPreferences.showAllUsersDepth = true;
                    texture2d = new ImiTexture2D(type, imiPreferences.depthResolution);
                    break;
                case ImiTexture2D.Type.MAIN_USER_COLOR:
                    if(imiPreferences.colorResolution.Size == imiPreferences.depthResolution.Size)
                    {
                        imiPreferences.showMainUserColor = true;
                    }else
                    {
                        Debug.Log("Getting Main User Color Texture needs the color resolution and depth resolution to be the same!");
                    }
                    texture2d = new ImiTexture2D(type, imiPreferences.depthResolution);
                    break;
                case ImiTexture2D.Type.SUB_USER_COLOR:
                    if(imiPreferences.colorResolution.Size == imiPreferences.depthResolution.Size)
                    {
                        imiPreferences.showSubUserColor = true;
                    }else
                    {
                        Debug.Log("Getting Sub User Color Texture needs the color resolution and depth resolution to be the same!");
                    }
                    texture2d = new ImiTexture2D(type, imiPreferences.depthResolution);
                    break;
                case ImiTexture2D.Type.MAIN_USER_DEPTH:
                    imiPreferences.showMainUserDepth = true;
                    texture2d = new ImiTexture2D(type, imiPreferences.depthResolution);
                    break;
                case ImiTexture2D.Type.SUB_USER_DEPTH:
                    imiPreferences.showSubUserDepth = true;
                    texture2d = new ImiTexture2D(type, imiPreferences.depthResolution);
                    break;
            }
            return texture2d;
        }

    }

    /// <summary>
    /// To Improve Performance, load data here
    /// This Class is designed to process data and store data
    /// Other ImiTexture2D should get their data here
    /// </summary>
    public class ImiTextureDataHolder
    {
        //color data is raw and passed from outside
        public byte[] colorDataRaw;
        public byte[] mainUserColorData;
        public byte[] subUserColorData;

        public byte[] depthData;
        public byte[] allUserDepthData;
        public byte[] mainUserDepthData;
        public byte[] subUserDepthData;

        private float[] usersHistogramMap;

        private bool hasAllUserDepth;
        private ImiPreferences imiPreferences;
        private ImiManager imiManager;

        private bool isInited;

        /// <summary>
        /// Stores All Texture States: ready or not ready
        /// If the texture is ready for apply
        /// Only when texture is not ready, process methods will be called
        /// </summary>
        public bool dataReady = false;

        private static ImiTextureDataHolder instance;
        private static object _lock = new object();
        public static ImiTextureDataHolder GetInstance()
        {
            if (instance == null)
            {
                lock (_lock)
                {
                    if (instance == null)
                    {
                        instance = new ImiTextureDataHolder();
                    }
                }
            }
            return instance;
        }

        private ImiTextureDataHolder()
        {
            imiPreferences = ImiPreferences.GetInstance();
            imiManager = ImiManager.GetInstance();
        }

        //Allocate all necessary memoreis here
        public void InitHolder()
        {
            if (isInited)
            {
                return;
            }
            isInited = true;
        }

        //Release all memories
        public void ReleaseHolder()
        {
            if (!isInited)
            {
                return;
            }

            depthData = null;
            allUserDepthData = null;
            mainUserColorData = null;
            subUserColorData = null;
            mainUserDepthData = null;
            subUserDepthData = null;

            isInited = false;
        }
        public void SetTextureUsed()
        {
            dataReady = false;
        }

        private void clearArrays()
        {
            //don't clear color data because it is raw data
            if (imiPreferences.showDepthOnly)
            {
                Array.Clear(depthData, 0, depthData.Length);
            }
            if (imiPreferences.showAllUsersDepth)
            {
                Array.Clear(allUserDepthData, 0, allUserDepthData.Length);
            }
            if (imiPreferences.showMainUserColor)
            {
                Array.Clear(mainUserColorData, 0, mainUserColorData.Length);
            }
            if (imiPreferences.showSubUserColor)
            {
                Array.Clear(subUserColorData, 0, subUserColorData.Length);
            }
            if (imiPreferences.showMainUserDepth)
            {
                Array.Clear(mainUserDepthData, 0, mainUserDepthData.Length);
            }
            if (imiPreferences.showSubUserDepth)
            {
                Array.Clear(subUserDepthData, 0, subUserDepthData.Length);
            }

        }
        private void initNecessaryArrays()
        {

            int depthMapSize = imiPreferences.depthResolution.Size;
            if (imiPreferences.showDepthOnly && depthData == null)
            {
                depthData = new byte[depthMapSize * 4];
            }

            if (imiPreferences.showAllUsersDepth && allUserDepthData == null)
            {
                allUserDepthData = new byte[depthMapSize * 4];
            }
            if (imiPreferences.showMainUserColor && mainUserColorData == null)
            {
                mainUserColorData = new byte[depthMapSize * 4];
            }
            if (imiPreferences.showSubUserColor && subUserColorData == null)
            {
                subUserColorData = new byte[depthMapSize * 4];
            }
            if (imiPreferences.showMainUserDepth && mainUserDepthData == null)
            {
                mainUserDepthData = new byte[depthMapSize * 4];
            }
            if (imiPreferences.showSubUserDepth && subUserDepthData == null)
            {
                subUserDepthData = new byte[depthMapSize * 4];
            }
            if (processDetphTexture() && usersHistogramMap == null)
            {
                usersHistogramMap = new float[65536];
            }

        }

        //if we need to process any depth texture
        private bool processDetphTexture()
        {
            return imiPreferences.showAllUsersDepth ||
                imiPreferences.showDepthOnly ||
                imiPreferences.showMainUserColor ||
                imiPreferences.showMainUserDepth ||
                imiPreferences.showSubUserColor ||
                imiPreferences.showSubUserDepth;
        }

        public void setColorDataRaw(byte[] colorDataRaw)
        {
            this.colorDataRaw = colorDataRaw;
        }

        //private long avg = 0;
        //private long num = 0;
        public bool ProcessRawData(ushort[] depthDataRaw,bool showDetail)
        {
            //If device is closed, simply return
            if (!imiManager.IsDeviceOpened())
            {
                return false;
            }

            if (dataReady)
            {
                return false;
            }

            //No need to process any depth texture
            if (!processDetphTexture())
            {
                dataReady = true;
                return true;
            }


            initNecessaryArrays();
            //Acquire Lock and start Processing

            clearArrays();

            //if(allUserDepthData == null)
            //{
            //    Debug.Log("All User Depth Data is Null!");
            //    dataReady = false;
            //    return false;
            //}
            //Log.d("Processing Depth Data...");


            int colorX = 0;
            int colorY = 0;
            int mainPlayerId = imiManager.GetControlPlayerId();
            int subPlayerId = imiManager.GetSubPlayerId();

            int depthWidth = imiPreferences.depthResolution.Width;
            int depthHeight = imiPreferences.depthResolution.Height;
            int colorWidth = imiPreferences.colorResolution.Width;
            int colorHeight = imiPreferences.colorResolution.Height;

            //Is depth stream contains user index
            bool hasUserIndex = imiPreferences.UserDepthAndSkeletonStream;
            if (showDetail)
            {
                for (int i = 0; i < imiPreferences.depthResolution.Size; i++)
                {
                    if (hasUserIndex)
                    {
                        usersHistogramMap[depthDataRaw[i] >> 3]++;
                    }
                    else
                    {
                        usersHistogramMap[depthDataRaw[i]]++;
                    }
                }

                for (int i = 1; i < usersHistogramMap.Length; i++)
                {
                    usersHistogramMap[i] += usersHistogramMap[i - 1];
                    usersHistogramMap[i - 1] = 1.0f - (usersHistogramMap[i - 1] / imiPreferences.depthResolution.Size);
                }

                usersHistogramMap[usersHistogramMap.Length - 1] = 1.0f - (usersHistogramMap[usersHistogramMap.Length - 1] / imiPreferences.depthResolution.Size);
            }


            //System.Diagnostics.Stopwatch watch = System.Diagnostics.Stopwatch.StartNew();
            for (int i = 0; i < imiPreferences.depthResolution.Size; i++)
            {
                if ((depthDataRaw[i] & 7) == 0 && !showDetail)
                {
                    continue;
                }

                //2. Depth Texture
                if (!hasUserIndex && imiPreferences.showDepthOnly)
                {
                    //Not User Index is Tracked, means datas contain "user" would not be processed
                    //handle depth data and return
                    int userDepth = depthDataRaw[i];
                    float histDepth = showDetail? usersHistogramMap[userDepth] : userDepth / 100;
                    depthData[i * 4] = (byte)(125 * histDepth);
                    depthData[i * 4 + 1] = (byte)(125 * histDepth);
                    depthData[i * 4 + 2] = (byte)(125 * histDepth);
                    depthData[i * 4 + 3] = 255;
                }
                else
                {
                    //2. Depth Texture with UserIndex
                    if (imiPreferences.showDepthOnly)
                    {
                        //Not User Index is Tracked, means datas contain "user" would not be processed
                        //handle depth data and return
                        int userDepth = depthDataRaw[i]>>3;
                        float histDepth = showDetail ? usersHistogramMap[userDepth] : userDepth / 100;
                        depthData[i * 4] = (byte)(125 * histDepth);
                        depthData[i * 4 + 1] = (byte)(125 * histDepth);
                        depthData[i * 4 + 2] = (byte)(125 * histDepth);
                        depthData[i * 4 + 3] = 255;
                    }

                    //3. Main User Color Texture
                    if (imiPreferences.showMainUserColor)
                    {
                        int tempPlayerId = depthDataRaw[i] & 7;
                        if (tempPlayerId > 0 && tempPlayerId == mainPlayerId )
                        {
                            //Debug.Log("Find player");
                            colorX = i % depthWidth;
                            colorY = i / depthWidth;

                            mainUserColorData[(colorY * colorWidth + colorX) * 4] = colorDataRaw[(colorY * colorWidth + colorX) * 3];
                            mainUserColorData[(colorY * colorWidth + colorX) * 4 + 1] = colorDataRaw[(colorY * colorWidth + colorX) * 3 + 1];
                            mainUserColorData[(colorY * colorWidth + colorX) * 4 + 2] = colorDataRaw[(colorY * colorWidth + colorX) * 3 + 2];
                            mainUserColorData[(colorY * colorWidth + colorX) * 4 + 3] = 255;
                        }

                    }


                    //4. Sub User Color Texture
                    if (imiPreferences.showSubUserColor)
                    {
                        int tempPlayerId = depthDataRaw[i] & 7;
                        if (tempPlayerId > 0 && tempPlayerId == subPlayerId)
                        {
                            colorX = i % depthWidth;
                            colorY = i / depthWidth;

                            subUserColorData[(colorY * colorWidth + colorX) * 4] = colorDataRaw[(colorY * colorWidth + colorX) * 3];
                            subUserColorData[(colorY * colorWidth + colorX) * 4 + 1] = colorDataRaw[(colorY * colorWidth + colorX) * 3 + 1];
                            subUserColorData[(colorY * colorWidth + colorX) * 4 + 2] = colorDataRaw[(colorY * colorWidth + colorX) * 3 + 2];
                            subUserColorData[(colorY * colorWidth + colorX) * 4 + 3] = 255;
                        }

                    }

                    //5. All User Depth Texture
                    if (imiPreferences.showAllUsersDepth || imiPreferences.showMainUserDepth || imiPreferences.showSubUserDepth)
                    {

                        int tempPlayerIndex = depthDataRaw[i] & 7;
                        int userDepth = depthDataRaw[i] >> 3;
                        float histDepth = showDetail ? usersHistogramMap[userDepth] : userDepth / 100;

                        if (imiPreferences.showAllUsersDepth)
                        {
                            if (tempPlayerIndex == mainPlayerId)
                            {
                                allUserDepthData[i * 4] = (byte)(255 * histDepth);
                                allUserDepthData[i * 4 + 1] = (byte)(0 * histDepth);
                                allUserDepthData[i * 4 + 2] = (byte)(215 * histDepth);
                                allUserDepthData[i * 4 + 3] = 255;

                            }
                            else if (tempPlayerIndex == subPlayerId)
                            {
                                allUserDepthData[i * 4] = (byte)(255 * histDepth);
                                allUserDepthData[i * 4 + 1] = (byte)(215 * histDepth);
                                allUserDepthData[i * 4 + 2] = (byte)(0 * histDepth);
                                allUserDepthData[i * 4 + 3] = 255;

                            }
                            else
                            {
                                allUserDepthData[i * 4] = (byte)(125 * histDepth);
                                allUserDepthData[i * 4 + 1] = (byte)(125 * histDepth);
                                allUserDepthData[i * 4 + 2] = (byte)(125 * histDepth);
                                allUserDepthData[i * 4 + 3] = 255;
                            }
                            /**
                            switch (tempPlayerIndex)
                            {
                                case 1:
                                    allUserDepthData[i * 4] = (byte)(255 * histDepth);
                                    allUserDepthData[i * 4 + 1] = (byte)(105 * histDepth);
                                    allUserDepthData[i * 4 + 2] = (byte)(180 * histDepth);
                                    allUserDepthData[i * 4 + 3] = 255;
                                    break;
                                case 2:
                                    allUserDepthData[i * 4] = (byte)(255 * histDepth);
                                    allUserDepthData[i * 4 + 1] = (byte)(0 * histDepth);
                                    allUserDepthData[i * 4 + 2] = (byte)(255 * histDepth);
                                    allUserDepthData[i * 4 + 3] = 255;
                                    break;
                                case 3:
                                    allUserDepthData[i * 4] = (byte)(255 * histDepth);
                                    allUserDepthData[i * 4 + 1] = (byte)(105 * histDepth);
                                    allUserDepthData[i * 4 + 2] = (byte)(180 * histDepth);
                                    allUserDepthData[i * 4 + 3] = 255;
                                    break;
                                case 4:
                                    allUserDepthData[i * 4] = (byte)(148 * histDepth);
                                    allUserDepthData[i * 4 + 1] = (byte)(0 * histDepth);
                                    allUserDepthData[i * 4 + 2] = (byte)(211 * histDepth);
                                    allUserDepthData[i * 4 + 3] = 255;
                                    break;
                                case 5:
                                    allUserDepthData[i * 4] = (byte)(0 * histDepth);
                                    allUserDepthData[i * 4 + 1] = (byte)(0 * histDepth);
                                    allUserDepthData[i * 4 + 2] = (byte)(255 * histDepth);
                                    allUserDepthData[i * 4 + 3] = 255;
                                    break;
                                case 6:
                                    allUserDepthData[i * 4] = (byte)(255 * histDepth);
                                    allUserDepthData[i * 4 + 1] = (byte)(215 * histDepth);
                                    allUserDepthData[i * 4 + 2] = (byte)(0 * histDepth);
                                    allUserDepthData[i * 4 + 3] = 255;
                                    break;
                                default:
                                    allUserDepthData[i * 4] = (byte)(125 * histDepth);
                                    allUserDepthData[i * 4 + 1] = (byte)(125 * histDepth);
                                    allUserDepthData[i * 4 + 2] = (byte)(125 * histDepth);
                                    allUserDepthData[i * 4 + 3] = 255;
                                    break;
                            }
                            **/

                        }
                        if (tempPlayerIndex > 0)
                        {

                            //6. Main User Depth Texture
                            if (imiPreferences.showMainUserDepth && tempPlayerIndex == (mainPlayerId))
                            {
                                mainUserDepthData[i * 4] = (byte)(0 * histDepth);
                                mainUserDepthData[i * 4 + 1] = (byte)(255 * histDepth);
                                mainUserDepthData[i * 4 + 2] = (byte)(255 * histDepth);
                                mainUserDepthData[i * 4 + 3] = 255;
                            }

                            //7. Sub User Depth Texture
                            if (imiPreferences.showSubUserDepth && tempPlayerIndex == (subPlayerId))
                            {
                                subUserDepthData[i * 4] = (byte)(0 * histDepth);
                                subUserDepthData[i * 4 + 1] = (byte)(255 * histDepth);
                                subUserDepthData[i * 4 + 2] = (byte)(127 * histDepth);
                                subUserDepthData[i * 4 + 3] = 255;
                            }
                        }

                    }
                }
            }
            //if (watch.ElapsedMilliseconds > 0)
            //{
            //    avg = (avg * num + watch.ElapsedMilliseconds) /( num + 1);
            //    num++;
            //    Log.d("average processing time : " + avg);
            //}
            //Texture is ready for apply
            dataReady = true;
            return true;
        }

    }
}
